#include "physic.hh"

const double Physic::TIMESTEP;
const double Physic::FRICTION;
const Vector3f Physic::GRAVITY = Vector3f (0, 0, -0.00006);

Physic::Physic (ObjVect& o)
 : obj_list_ (o),
   mov_obj_ (),
   unmov_obj_ (),
   old_obj_pos_ (),
   final_obj_mov_ ()
{
  pf_ = new PhysicFunctor;
  o_rts_ = OctreeRts::Instance ();
  // FIXME : Not sure => Vector3f::zero is static, don't work with &
  // FIXME: 8 points are needed to do the 6 faces of the cube, quick and dirty.
  Vector3f w1 = Vector3f:: zero ();
  Vector3f w2 = Vector3f (o_rts_->size (), 0, 0);
  Vector3f w3 = Vector3f (0, o_rts_->size (), 0);
  Vector3f w4 = Vector3f (0, 0, o_rts_->size ());
  Vector3f w5 = Vector3f (o_rts_->size (), o_rts_->size (), 0);
  Vector3f w6 = Vector3f (o_rts_->size (), 0, o_rts_->size ());
  Vector3f w7 = Vector3f (0, o_rts_->size (), o_rts_->size ());
  walls_.push_back (Plane (w1, w2, w3));
  walls_.push_back (Plane (w1, w2, w4));
  walls_.push_back (Plane (w1, w3, w4));
  walls_.push_back (Plane (w5, w2, w6));
  walls_.push_back (Plane (w3, w5, w7));
  walls_.push_back (Plane (w4, w6, w7));
}

// FIXME: Add weight to objects.
void
Physic::update (double elaps)
{
  Vector3f new_pos;
  Vector3f movement;
  Vector3f tmp;

  //PIERRE
  for (std::list<FpsZone*>::const_iterator it = zone_.begin ();
       it != zone_.end (); ++it)
  {
    for (ObjVect::iterator jt = obj_list_.begin ();
	 jt != obj_list_.end (); ++jt)
    {
      if ((*it)->intersect (jt->second->main_bounding_get ()))
	(*it)->interact (jt->second);
    }
  }
  //END PIERRE

  mov_obj_.clear ();

  // 1st step: Processing of the new positions.
  // FIXME: Processing of angular speed !
  for (ObjVect::iterator it = obj_list_.begin ();
       it != obj_list_.end (); ++it)
  {
    // FIXME: To enable.
    /*tmp = GRAVITY;
    tmp *= (1 / it->second->mass ());
    it->second->accel_add (tmp);*/
    tmp = it->second->speed_get () + (it->second->accel () * elaps);
    tmp -= (it->second->speed_get () * FRICTION);
    // FIXME: Problem with the acceleration ! You have to cut it (floor).
    if (tmp.get_norme () < 0.0005)
    {
      unmov_obj_.insert (it->second);
      tmp = Vector3f::zero ();
    }
    else
    {
      // Only if moving object.
      mov_obj_.insert (it->second);
      o_rts_->delete_obj (it->second);
      movement = tmp * elaps + it->second->accel () * elaps * elaps * 0.5;
      old_obj_pos_.insert (std::make_pair (it->second, it->second->pos_get ()));
      it->second->move (movement);
      o_rts_->add_obj (it->second);
    }
    // We need to set speed in any case.
    it->second->speed_set (tmp);
  }

  // We iterate on the moving object list.
  for (std::set<Object*>::iterator it = mov_obj_.begin ();
       it != mov_obj_.end (); ++it)
  {
    pf_->object_set (*it);
    o_rts_->accept (pf_);
    std::list<Object*>* inter_list = pf_->intersect_list ();
    if (inter_list->size ())
      do_collisions (it, inter_list);
    // Collisions with walls.
    wall_collision (it);
  }
  old_obj_pos_.clear ();

  for (std::set<Object*>::iterator it = mov_obj_.begin ();
       it != mov_obj_.end (); ++it)
  {
    // FIXME: Handle final speed.
    o_rts_->delete_obj (*it);
    (*it)->move (final_obj_mov_[*it]);
    o_rts_->add_obj (*it);
  }
  final_obj_mov_.clear ();
}

void Physic::do_collisions (std::set<Object*>::iterator i, std::list<Object*>* inter_list)
{
  for (objIterator it = inter_list->begin (); it != inter_list->end (); ++it)
  {
    // TODO : Detect where is the collision !
    // TODO : Think about the interface for event (touched object, etc ...)
    // TODO : Interpolation linéaire si distance parcouru > taille de l'objet = opti
    // TODO: % de recalage de chaque objet. (norme des deux vitesses)
    if ((**i) () == SPHERE && (**it) () == SPHERE)
      sphere_sphere_collision (i, *it);
    else if ((**i) () == BOX && (**it) () == BOX)
      cube_cube_collision (i, *it);
    else
      sphere_cube_collision (i, *it);
 }
}

void
Physic::wall_collision (std::set<Object*>::iterator i)
{
  std::list<Plane>::iterator it;
  for (it = walls_.begin (); it != walls_.end (); ++it)
  {
    if ((*i)->intersectWall (*it))
    {
      if ((**i) () == SPHERE)
        plane_sphere_collision (i, *it);
    }
  }
}

void
Physic::plane_sphere_collision (std::set<Object*>::iterator i, Plane& p)
{
  // Realining.
  Vector3f pos = (*i)->pos_get ();
  // Ratio for Thales.
  float ratio = (*i)->size_get () (0) / p.distanceToPoint (pos);
  Vector3f direction = (*i)->speed_get () / (*i)->speed_get ().get_norme ();

  Vector3f intersectP =
    p.intersectionPoint (pos, (*i)->speed_get ());

  float distanceFromCollision = (intersectP - pos).get_norme ();
  float res = distanceFromCollision * ratio - distanceFromCollision + 0.01;
  final_obj_mov_[*i] = direction * (-res);

  // Bouncing.
  // FIXME: Not sure.

  // Should be good to round the speed when colliding with the floor.

  Vector3f normal (p.a (), p.b (), p.c ());
  Vector3f finalSpeed = (*i)->speed_get ();
  float scal = (*i)->speed_get ().scalar(normal);
  scal *= 2;
  finalSpeed -= normal * scal;
  (*i)->speed_set (finalSpeed * 0.5);
}

void
Physic::sphere_sphere_collision (std::set<Object*>::iterator i, Object* other)
{
  Vector3f other_pos = other->pos_get ();
  Object* obj = *i;
  Vector3f pos = obj->pos_get ();
  double ratio = (other_pos - pos).get_norme ();
  Vector3f norme = (other_pos - pos) / ratio;
  double diff = ((*i)->size_get () (0) +
                 other->size_get () (0)) -
                 (other_pos - pos).get_norme () + 0.01;

  Vector3f res = norme * (diff);

  // Realining.
  final_obj_mov_[*i] -= res;

  // Elastic collision.
  double nx = (other_pos (0) - pos (0)) /
              (obj->size_get () (0) +
               other->size_get () (0));
  double ny = (other_pos (1) - pos (1)) /
              (obj->size_get () (0) +
               other->size_get () (0));
  double nz = (other_pos (2) - pos (2)) /
              (obj->size_get () (0) +
               other->size_get () (0));
  double gx = -ny;
  double gy = nx;
  double gz = nz;

  double v1n = nx * (obj->speed_get ()) (0) +
               ny * (obj->speed_get ()) (1) +
               nz * (obj->speed_get ()) (2);

  double v1g = gx * (obj->speed_get ()) (0) +
               gy * (obj->speed_get ()) (1) +
               gz * (obj->speed_get ()) (2);

  double v2n = nx * (other->speed_get ()) (0) +
               ny * (other->speed_get ()) (1) +
               nz * (other->speed_get ()) (2);

  double v2g = gx * (other->speed_get ()) (0) +
               gy * (other->speed_get ()) (1) +
               gz * (other->speed_get ()) (2);


  Vector3f speed_obj1 = Vector3f (nx * v2n + gx * v1g,
                                  ny * v2n + gy * v1g,
                                  nz * v2n + gz * v1g);
  Vector3f speed_obj2 = Vector3f (nx * v1n + gx * v2g,
                                  ny * v1n + gy * v2g,
                                  nz * v1n + gz * v2g);

  obj->speed_set (speed_obj1);
  other->speed_set (speed_obj2);
}

void
Physic::sphere_cube_collision (std::set<Object*>::iterator i, Object* other)
{
  i = i;
  other = other;
}

void
Physic::cube_cube_collision (std::set<Object*>::iterator i, Object* other)
{
  i = i;
  other = other;
}

const std::set<Object*>&
Physic::mov_obj () const
{
  return mov_obj_;
}

const std::set<Object*>&
Physic::unmov_obj () const
{
  return unmov_obj_;
}

Physic::~Physic ()
{
}

